#ifndef MODEL_FORWARD_KINEMATICS_H
#define MODEL_FORWARD_KINEMATICS_H

#include "model/model_impl/basic_data_structure.hpp"
#include "robot_model.hpp"

namespace ZORAL
{
    class ForwardKinematics
    {
    private:
        RobotModel *rm_;

    public:
        explicit ForwardKinematics(RobotModel *rm)
        {
            rm_ = rm;
        }

//        explicit ForwardKinematics(const std::string &robot_name, const std::string &package_path)
//        {
//            rm_ = new RobotModel(robot_name, package_path);
//        }
        RobotModel *getRobotModel()
        {
            return rm_;
        }

        int forwardPosition(const VectorXd &q, HomoTrans &T_out);
        int forwardPositions(const VectorXd &q, const bool &is_relate_to_base, std::vector<HomoTrans> &T_all, HomoTrans &T_out);
    };
} // namespace ZORAL

#endif